
#include <ESP32Servo.h>  // ESP32系列专用舵机库（含C3）
#include <WiFi.h>        // ESP32-C3 WiFi库（替换ESP8266WiFi.h）
#include <WebServer.h>   // ESP32-C3 Web服务器库（替换ESP8266WebServer.h）

#include <WebSocketMCP.h>  // 安装 xiaozhi_mcp库
#include <ArduinoJson.h>  // ESP32环境需单独引入JSON库
// -------------------------- 1. 核心参数配置 --------------------------
// WiFi网络信息（改为你的WiFi名称和密码）
const char* WIFI_SSID = "joe";
const char* WIFI_PASSWORD = "joejoejoe";
                           
const char* mcpEndpoint = "wss://api.xiaozhi.me/mcp/?token=token"; //token需要到xiaozhi.me上复制替换
WebSocketMCP mcpClient;

// 舵机配置（ESP32-C3用GPIO编号，推荐GPIO5/6/7/8，避免下载引脚冲突）
Servo servoFL; // 前左舵机
Servo servoFR; // 前右舵机
Servo servoBL; // 后左舵机
Servo servoBR; // 后右舵机
const int PIN_FL = 5;   // 前左舵机 -> GPIO5
const int PIN_FR = 6;   // 前右舵机 -> GPIO6
const int PIN_BL = 7;   // 后左舵机 -> GPIO7
const int PIN_BR = 8;   // 后右舵机 -> GPIO8

// 动作参数（保持原逻辑，可根据舵机实际角度调整）
const int ANGLE_NEUTRAL = 90;    // 站立中立位（多数舵机中间角度）
const int ANGLE_STEP = 30;       // 动作偏移角度（根据舵机行程调整）
const int ANGLE_LIE_DOWN = 150;  // 趴下角度（需实际测试校准）
const int DELAY_SHORT = 100;     // 短延迟（舵机动作响应时间）
const int DELAY_LONG = 800;      // 长延迟（复杂动作间隔）
const int WAVE_TIMES = 3;        // 招手次数
const int SWING_TIMES = 4;       // 扭动/摇摆次数

// Web服务器对象（默认端口80，ESP32-C3支持）
WebServer server(80);

void initServos();        // 舵机初始化
void connectWiFi();       // WiFi连接
void initWebServer();     // 网页服务器初始化
void standUp();           // 站起来
void walk(int direction, int steps = 5);  // 步行（带默认参数）
void turn(int direction, int steps = 5);  // 转向（带默认参数）
void waveHand();          // 招手
void twistBody(int times = SWING_TIMES);  // 扭动身体
void swingForwardBack(int times = SWING_TIMES);  // 前后摇摆
void lieDown();           // 趴下
void moveAllToNeutral();  // 舵机回中立位
void control(int type);   // 动作控制分发
void onConnectionStatus(bool connected);  // MCP连接回调
WebSocketMCP::ToolResponse dogControl(const String& params);  // MCP工具回调
void registerMcpTools();  // MCP工具注册
String createWebPage();   // 生成网页HTML


// -------------------------- 2. 初始化函数 --------------------------
void setup() {
  Serial.begin(115200);  // 串口调试（波特率115200）
  while (!Serial);       // 等待串口监视器连接（可选，方便调试）
  
  initServos();       // 初始化舵机（适配ESP32-C3）
  connectWiFi();      // 连接WiFi（ESP32-C3逻辑）
  initWebServer();    // 初始化网页服务器
  standUp();          // 开机默认站起来
  
  
  if(mcpClient.begin(mcpEndpoint,onConnectionStatus)){
    Serial.println("MCP 客户端初始化成功");
  }else{
    
    Serial.println("MCP 客户端初始化失败");
  }
  delay(1000);
  Serial.print("网页控制地址：");
  Serial.println(WiFi.localIP()); // 打印ESP32-C3的IP（用于浏览器访问）
}

void loop() {
  server.handleClient(); // 持续监听网页请求（ESP32-C3兼容）
  mcpClient.loop();
}


// MCP连接状态回调（保持原逻辑）
void onConnectionStatus(bool connected) {
  if (connected) {
    Serial.println("MCP 已连接到服务器");
    // 连接成功后注册工具
    registerMcpTools();
  } else {
    Serial.println("MCP 已断开与服务器的连接");
  }
}

// MCP工具回调：小车控制（保持原逻辑）
WebSocketMCP::ToolResponse dogControl(const String& params) {
  Serial.println("原始参数: " + params);
  
  // 创建JSON文档（大小根据实际需求调整）
  DynamicJsonDocument doc(2048);
  
  // 解析JSON字符串
  DeserializationError error = deserializeJson(doc, params);
  
  // 检查解析是否成功
  if (error) {
    String errorMsg = "JSON解析失败: " + String(error.c_str());
    Serial.println(errorMsg);
    return WebSocketMCP::ToolResponse(true, errorMsg);
  }
  
  // 检查是否为JSON对象
  if (!doc.is<JsonObject>()) {
    String errorMsg = "JSON格式错误，不是对象类型";
    Serial.println(errorMsg);
    return WebSocketMCP::ToolResponse(true, errorMsg);
  }
  
  // 从JSON对象中获取值（不存在则使用默认值）
  int type = doc["type"] | 0;
  
  Serial.println("解析参数 - type: " + String(type));
  
  control(type);
  return WebSocketMCP::ToolResponse(false, "指令已发送");
}

// 注册MCP工具（保持原逻辑）
void registerMcpTools() {
  // 注册小车控制工具
  mcpClient.registerTool(
    "dog_control",
    "控制机器狗运动",
    "{\"type\":\"object\",\"properties\":{\"type\":{\"type\":\"int\",\"description\":\"小狗的动作执行 1:向前走 2.先后走 3.向左转 4.向右转 5.招手 6.趴下 7.站起来 8.扭动身体 9.前后摇动身体 10.坐下 11.抻懒腰 \"}},\"required\":[\"type\"]}",
    dogControl
  );
}


// -------------------------- 3. 舵机控制函数（核心适配） --------------------------
// 舵机初始化（适配ESP32-C3：添加频率和脉冲宽度配置）
void initServos() {
  // 配置舵机：50Hz（标准舵机频率）、脉冲宽度500-2400μs（覆盖多数舵机行程）
  servoFL.setPeriodHertz(50);
  servoFR.setPeriodHertz(50);
  servoBL.setPeriodHertz(50);
  servoBR.setPeriodHertz(50);
  
  // 绑定舵机引脚并设置脉冲范围（attach(引脚, 最小脉冲, 最大脉冲)）
  servoFL.attach(PIN_FL, 500, 2400);
  servoFR.attach(PIN_FR, 500, 2400);
  servoBL.attach(PIN_BL, 500, 2400);
  servoBR.attach(PIN_BR, 500, 2400);
  
  moveAllToNeutral(); // 所有舵机回中立位
  delay(1000);
  Serial.println("舵机初始化完成（ESP32-C3）");
}

// 所有舵机回中立位（逻辑不变）
void moveAllToNeutral() {
  servoFL.write(ANGLE_NEUTRAL);
  servoFR.write(ANGLE_NEUTRAL);
  servoBL.write(ANGLE_NEUTRAL);
  servoBR.write(ANGLE_NEUTRAL);
}

// -------------------------- 4. 动作函数（逻辑不变，仅适配舵机库） --------------------------
void fr(int direction, int angle){
  int _angle= angle;
  if(direction==1){
    _angle= angle*-1;
  }
    servoFR.write(ANGLE_NEUTRAL - _angle);
}
void fl(int direction,int angle){
  int _angle= angle;
  if(direction==1){
    _angle= angle*-1;
  }
    servoFL.write(ANGLE_NEUTRAL + _angle);
}

void br(int direction,int angle){
  int _angle= angle;
  if(direction==1){
    _angle= angle*-1;
  }
    servoBR.write(ANGLE_NEUTRAL - _angle);
}

void bl(int direction,int angle){
  int _angle= angle;
  if(direction==1){
    _angle= angle*-1;
  }
    servoBL.write(ANGLE_NEUTRAL + _angle);
}
void control(int type){
  switch(type){
    case 1:
       walk(1,5);
       break;
    case 2:
       walk(-1,5);
       break;
    case 3:
       turn(1,5);
       break;
    case 4:
       turn(-1,5);
       break;
    case 5:
       waveHand();
       break;
       
    case 6:
       lieDown();
       break;
    
    case 7:
       standUp();
       break;
    
    case 8:
       twistBody(SWING_TIMES);
       break;
    
    case 9:
       swingForwardBack(SWING_TIMES);
       break;
    
    case 10:
       sitDown();
       break;
       
    case 11:
       lazily();
       break;
    default:
       break;
  }
}
// 向前走（2步）
void walk(int direction,int steps) {
  int angle = ANGLE_STEP;
  if(direction==1){
    angle=ANGLE_STEP * -1;
  }
  Serial.println("执行：向前走");
  for (int i = 0; i < steps; i++) {
    fl(-1,angle);
    br(-1,angle);
    delay(DELAY_SHORT);
    fr(1,angle);
    bl(1,angle);
    delay(DELAY_SHORT);
    fl(0,0);
    br(0,0);
    delay(DELAY_SHORT);
    fr(0,0);
    bl(0,0);
    delay(DELAY_SHORT);
    fr(-1,angle);
    bl(-1,angle);
    delay(DELAY_SHORT);
    fl(1,angle);
    br(1,angle);
    delay(DELAY_SHORT);
    delay(DELAY_SHORT);
    fr(0,0);
    bl(0,0);
    delay(DELAY_SHORT);
    fl(0,0);
    br(0,0);
    delay(DELAY_SHORT);

  }
}

// 左转
void turn(int direction,int steps) {
  int angle=ANGLE_STEP;
  if(direction==1){
    angle=ANGLE_STEP*-1;
  }
  for(int i=0;i<steps;i++){
    fl(1,angle);
    br(-1,angle);
    delay(DELAY_SHORT);
    fr(1,angle);
    bl(-1,angle);
    delay(DELAY_SHORT);
    fl(0,0);
    br(0,0);
    delay(DELAY_SHORT);
    fr(0,0);
    bl(0,0);
    delay(DELAY_SHORT);
  }
}


// 招手（仅前右舵机动作）
void waveHand() {
  Serial.println("执行：招手");
  for (int i = 0; i < WAVE_TIMES; i++) {
    servoFR.write(ANGLE_NEUTRAL - ANGLE_STEP * 1.2);
    delay(500);
    servoFR.write(ANGLE_NEUTRAL + ANGLE_STEP * 1.2);
    delay(500);
  }
  servoFR.write(ANGLE_NEUTRAL);
}

// 扭动身体
void twistBody(int times) {
  Serial.println("执行：扭动");
  for (int i = 0; i < times; i++) {
    fl(-1,ANGLE_STEP);
    fr(1,ANGLE_STEP);
    bl(-1,ANGLE_STEP);
    br(1,ANGLE_STEP);
    delay(DELAY_SHORT);
    fl(1,ANGLE_STEP);
    fr(-1,ANGLE_STEP);
    bl(1,ANGLE_STEP);
    br(-1,ANGLE_STEP);
    delay(DELAY_SHORT);
  }
  moveAllToNeutral();
}

// 前后摇摆
void swingForwardBack(int times) {
  Serial.println("执行：前后摇摆");
  for (int i = 0; i < times; i++) {    
    fl(1,ANGLE_STEP);
    fr(1,ANGLE_STEP);
    bl(1,ANGLE_STEP);
    br(1,ANGLE_STEP);
    delay(DELAY_SHORT);
    fl(-1,ANGLE_STEP);
    fr(-1,ANGLE_STEP);
    bl(-1,ANGLE_STEP);
    br(-1,ANGLE_STEP);
    delay(DELAY_SHORT);
  }
  moveAllToNeutral();
}

// 趴下
void lieDown() {
  Serial.println("执行：趴下");
  fl(1,ANGLE_LIE_DOWN);
  fr(1,ANGLE_LIE_DOWN);
  bl(-1,ANGLE_LIE_DOWN);
  br(-1,ANGLE_LIE_DOWN);
}

// 站起来
void standUp() {
  Serial.println("执行：站起来");
    fl(0,0);
    fr(0,0);
    bl(0,0);
    br(0,0);
}

// 坐下
void sitDown() {
  Serial.println("执行：坐下");
  fl(0,0);
  fr(0,0);
  bl(1,ANGLE_LIE_DOWN);
  br(1,ANGLE_LIE_DOWN);
}
// 抻懒腰
void lazily() {
  Serial.println("执行：抻懒腰");
  fl(0,0);
  fr(0,0);
  bl(-1,ANGLE_LIE_DOWN);
  br(-1,ANGLE_LIE_DOWN);
    delay(DELAY_LONG);
  moveAllToNeutral();
}
// -------------------------- 5. WiFi连接（适配ESP32-C3） --------------------------
void connectWiFi() {
  Serial.print("正在连接WiFi：");
  Serial.println(WIFI_SSID);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD); // ESP32-C3 WiFi初始化逻辑
  
  // 等待WiFi连接（最多等待15秒，避免无限阻塞）
  int waitTime = 0;
  while (WiFi.status() != WL_CONNECTED && waitTime < 15) {
    delay(500);
    Serial.print(".");
    waitTime++;
  }
  
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nWiFi连接成功！");
  } else {
    Serial.println("\nWiFi连接失败，请检查SSID/密码或信号！");
    while (1); // 连接失败则暂停程序（可根据需求修改为重试逻辑）
  }
}

// -------------------------- 6. 网页服务器（适配ESP32-C3 WebServer） --------------------------
// 初始化网页路由（绑定按钮指令）
void initWebServer() {
  // 根路由：访问IP时返回控制网页
  server.on("/", HTTP_GET, []() {
    server.send(200, "text/html", createWebPage()); // 发送HTML页面
  });
  
  // 动作指令路由（点击按钮触发对应动作）
  server.on("/forward", HTTP_GET, []() {
    walk(1,5);
    server.send(200, "text/plain", "执行向前走");
  });
  
  server.on("/backward", HTTP_GET, []() {
    walk(-1,5);
    server.send(200, "text/plain", "执行向后走");
  });

  
  server.on("/turnLeft", HTTP_GET, []() {
    turn(1,5);
    server.send(200, "text/plain", "执行左转");
  });
  
  server.on("/turnRight", HTTP_GET, []() {
    turn(-1,5);
    server.send(200, "text/plain", "执行右转");
  });
  
  server.on("/wave", HTTP_GET, []() {
    waveHand();
    server.send(200, "text/plain", "执行招手");
  });
  
  server.on("/twist", HTTP_GET, []() {
    twistBody(SWING_TIMES);
    server.send(200, "text/plain", "执行扭动");
  });
  
  server.on("/swing", HTTP_GET, []() {
    swingForwardBack(SWING_TIMES);
    server.send(200, "text/plain", "执行前后摇摆");
  });
  
  server.on("/lieDown", HTTP_GET, []() {
    lieDown();
    server.send(200, "text/plain", "执行趴下");
  });
  
  server.on("/standUp", HTTP_GET, []() {
    standUp();
    server.send(200, "text/plain", "执行站起来");
  });
  server.on("/sitDown", HTTP_GET, []() {
    sitDown();
    server.send(200, "text/plain", "执行坐下");
  });
  server.on("/lazily", HTTP_GET, []() {
    lazily();
    server.send(200, "text/plain", "执行抻懒腰");
  });
  
  server.begin(); // 启动Web服务器（ESP32-C3兼容）
  Serial.println("网页服务器已启动（ESP32-C3）");
}

// 生成网页界面（HTML+CSS，支持手机/电脑访问，逻辑不变）
String createWebPage() {
  String html = R"=====(
    <!DOCTYPE html>
    <html>
    <head>
      <meta charset="UTF-8">
      <meta name="viewport" content="width=device-width, initial-scale=1.0">
      <title>机器狗控制中心（ESP32-C3）</title>
      <style>
        body { font-family: Arial; max-width: 600px; margin: 20px auto; padding: 0 10px; }
        h1 { text-align: center; color: #333; }
        .btn-group { display: grid; grid-template-columns: 1fr 1fr; gap: 15px; margin-top: 30px; }
        .action-btn { 
          padding: 20px 10px; 
          font-size: 18px; 
          background-color: #2196F3; /* 调整按钮颜色为ESP32主题蓝 */
          color: white; 
          border: none; 
          border-radius: 8px; 
          cursor: pointer; 
        }
        .action-btn:hover { background-color: #1976D2; }
      </style>
    </head>
    <body>
      <h1>机器狗动作控制</h1>
      <div class="btn-group">
        <button class="action-btn" onclick="sendCmd('forward')">向前走</button>
        <button class="action-btn" onclick="sendCmd('backward')">向后走</button>
        <button class="action-btn" onclick="sendCmd('turnLeft')">左转</button>
        <button class="action-btn" onclick="sendCmd('turnRight')">右转</button>
        <button class="action-btn" onclick="sendCmd('wave')">招手</button>
        <button class="action-btn" onclick="sendCmd('twist')">扭动</button>
        <button class="action-btn" onclick="sendCmd('swing')">前后摇摆</button>
        <button class="action-btn" onclick="sendCmd('lieDown')">趴下</button>
        <button class="action-btn" onclick="sendCmd('standUp')">站起来</button>
        <button class="action-btn" onclick="sendCmd('sitDown')">坐下</button>
        <button class="action-btn" onclick="sendCmd('lazily')">抻懒腰</button>
      </div>

      <script>
        // 发送指令到ESP32-C3（通过HTTP GET请求）
        function sendCmd(cmd) {
          fetch('/' + cmd)
            .then(response => response.text())
            .then(text => console.log('执行结果：', text))
            .catch(err => console.error('指令发送失败：', err));
        }
      </script>
    </body>
    </html>
  )=====";
  return html;
}